package process;

import Jama.Matrix;

public class KalmanFilter {

	private Matrix x;
	private Matrix F;
	private Matrix P;
	private Matrix H;
	private Matrix R; // sigmaM
	private Matrix I;
	private Matrix W; // sigmaW

	private void setF(double t) {
		F.set(0, 0, 1);
		F.set(1, 1, 1);
		F.set(2, 2, 1);
		F.set(0, 1, t);
		F.set(0, 2, t * t / 2);
		F.set(1, 2, t);
	}

	public KalmanFilter(double sM, double sW) {
		x = new Matrix(new double[3][1]);
		double[][] p = new double[3][3];
		p[0][0] = p[1][1] = p[2][2] = 1000;
		P = new Matrix(p);
		F = new Matrix(new double[3][3]);
		H = new Matrix(new double[1][3]);
		H.set(0, 0, 1);
		R = new Matrix(new double[1][1]);
		R.set(0, 0, sM);
		I = Matrix.identity(3, 3);
		W = (Matrix.identity(3, 3)).times(sW);

	}

	public double[] filter(double time, double px) {
		// 1predict
		setF(time);
		x = F.times(x);
		P = ((F.times(P)).times(F.transpose())).plus(W);

		// 2update
		double xy = px - x.get(0, 0);
		double S = P.get(0, 0) + R.get(0, 0);
		Matrix K = new Matrix(new double[3][1]);
		K = (P.getMatrix(0, 2, 0, 0).times(1 / S));
		x = x.plus(K.times(xy));
		P = (I.minus(K.times(H))).times(P);
		double[] result = new double[3];
		result[0] = x.get(0, 0);
		result[1] = x.get(1, 0);
		result[2] = x.get(2, 0);
		return result;
	}
}
